#include "PID.h"

PID L_SpeedPID ={0};
PID R_SpeedPID ={0};

/****************************PID参数初始化**************************************
函数：  void PID_int(void)
参数：  void
说明：  PID每个环参数初始化
返回值：void
作者：  heqiu
********************************************************************************/
void PID_Date_Init(void)
{
	
	L_SpeedPID.Kp=1;   
	L_SpeedPID.Ki=81;       
	L_SpeedPID.Kd=0;   
                                                             
	R_SpeedPID.Kp=1;   
	R_SpeedPID.Ki=85;    
	R_SpeedPID.Kd=0;      
}

/************************************************
函数名：PID_Init(PID *sptr)
功  能：PID参数初始化
参  数：
返回值：void
************************************************/
void PID_Init(PID *sptr)
{
    sptr->SumError=0;
    sptr->LastError=0;
    sptr->LLastError=0;

    sptr->Kp=0;
    sptr->Ki=0;
    sptr->Kd=0;
}

/************************************************
Loca_Contral(PID *sptr,int16_t Setpoint,int16_t Turepoint)
功  能：位置式PID控制
参  数：PID *sptr,int16_t Setpoint,int16_t Turepoint
返回值：float 
************************************************/
int16_t Loca_Contral(PID*sptr,int16_t Setpoint,int16_t Turepoint)
{
    int16_t iError,dError;
    int16_t output;

    iError=Setpoint-Turepoint;  //偏差
    sptr->SumError+=iError;     //积分(采样时间很短时，用一阶差分代替一阶微分，用累加代替积分)
    dError=(int16_t)(iError-(sptr->LastError));     //微分
    sptr->LastError=iError;

    if(sptr->SumError>2500) sptr->SumError=2500;   //积分限幅
    if(sptr->SumError<-2500) sptr->SumError=-2500;

    output=(int16_t)(sptr->Kp*iError  //比例项
          +(sptr->Ki*sptr->SumError)//积分项
          +sptr->Kd*dError);        //微分项

    return(output);
}